#!/usr/bin/env python3
import rospy
import time
import matplotlib.pyplot as plt
from actionlib_msgs.msg import GoalStatusArray
from move_base_msgs.msg import MoveBaseActionGoal
from nav_msgs.msg import Odometry
import numpy as np

class NavigationTimeRecorder:
    def __init__(self):
        rospy.init_node('navigation_time_recorder')
        

        self.start_time = None
        self.end_time = None
        self.is_navigating = False
        

        self.path_points = []
        

        rospy.Subscriber('/move_base/status', GoalStatusArray, self.status_callback)
        rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_callback)
        

        rospy.Subscriber('/odom', Odometry, self.odom_callback)
        
        rospy.loginfo("导航时间记录器已启动")
        
    def goal_callback(self, msg):

        if not self.is_navigating:
            self.start_time = time.time()
            self.is_navigating = True
            self.path_points = []  # 清空路径点
            rospy.loginfo("导航开始")
    
    def odom_callback(self, msg):

        if self.is_navigating:
            x = msg.pose.pose.position.x
            y = msg.pose.pose.position.y
            self.path_points.append((x, y))
    
    def status_callback(self, msg):

        if not self.is_navigating or self.start_time is None:
            return
            

        if len(msg.status_list) == 0:
            return
            

        current_status = msg.status_list[-1].status
        

        if current_status == 3:
            self.end_time = time.time()
            navigation_time = self.end_time - self.start_time
            rospy.loginfo(f"导航成功！用时: {navigation_time:.2f} 秒")
            

            self.plot_path()
            

            self.is_navigating = False
            self.start_time = None
            

        elif current_status == 4:
            self.end_time = time.time()
            navigation_time = self.end_time - self.start_time
            rospy.loginfo(f"导航中止！用时: {navigation_time:.2f} 秒")
            

            self.plot_path()
            

            self.is_navigating = False
            self.start_time = None
    
    def plot_path(self):

        if not self.path_points:
            rospy.loginfo("没有路径点可绘制")
            return
            

        x_points = [point[0] for point in self.path_points]
        y_points = [point[1] for point in self.path_points]
        

        plt.figure(figsize=(10, 8))
        

        plt.plot(x_points, y_points, '-', color='blue', linewidth=2, label='行驶路径')
        

        plt.scatter(x_points[0], y_points[0], color='green', s=100, marker='o', label='起点')
        plt.scatter(x_points[-1], y_points[-1], color='red', s=100, marker='o', label='终点')
        

        plt.title('机器人导航路径')
        plt.xlabel('X 坐标 (米)')
        plt.ylabel('Y 坐标 (米)')
        plt.grid(True)
        plt.legend()
        plt.axis('equal') 
        

        timestamp = time.strftime("%Y%m%d-%H%M%S")
        filename = f"navigation_path_{timestamp}.png"
        plt.savefig(filename)
        rospy.loginfo(f"路径图已保存为: {filename}")
        

        # plt.show()

if __name__ == '__main__':
    try:

        plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"]
        recorder = NavigationTimeRecorder()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass